clear all;clc;close all;
mex cec14_func.cpp -DWINDOWS

bool_Plot = true;
Dimension=10;
Xmin=-100;
Xmax=100;
Particle_Number=50;
Max_Gen=3000;
varargin = 25;
fhd=str2func('cec14_func');
lowerbound = Xmin;
upperbound = Xmax;

me = Max_Gen;
ps = Particle_Number;
D = Dimension;
pos = InitializePopulation(D,ps,Xmin,Xmax);

num_FClevel = 5;
prob_forage = 0.5;
size_each_level = repmat(floor(ps/num_FClevel),1,num_FClevel);
size_each_level(num_FClevel) = ps - (size_each_level(1)*(num_FClevel-1));
for n = 1 : ps
    if(n<=ps-size_each_level(num_FClevel))
        if(mod(n,size_each_level(1)) == 0)
            level_species(n,1) = n/size_each_level(1);
        else
            level_species(n,1) = floor(n/size_each_level(1))+1;
        end
    else
        level_species(n,1) = num_FClevel;
    end
end
lb_level(1,1) = 1;
ub_level(1,1) = size_each_level(1);
for l = 2 : num_FClevel
    lb_level(l,1) = sum(size_each_level(1,1:l-1))+1;
    ub_level(l,1) = sum(size_each_level(1,1:l));
end

[pub, plb, vub, vlb] = SetBound(D,ps,lowerbound,upperbound);
% pos = VRmin+(VRmax-VRmin).*rand(ps,D);

fitcount = ps;
vel = zeros(ps,D); % initialize the velocity of the particles

fitness = feval(fhd,pos',varargin);
[fitbest,gbestindex] = min(fitness);
fitworst = max(fitness);
gbest = pos(gbestindex,:);
gbestval = fitbest;
if(bool_Plot)
    figure(8)
    haxes = plot( 0 , 0 );
    XArray = [1];
    YArray = [gbestval];
    title('FCO');
end

for iter = 2:me    
    if(fitbest == fitworst)
        mass = repmat(1/ps,ps,1);
    else
        mass = (fitness'-fitworst)/(fitbest-fitworst);
    end
%     mass = mass / sum(mass);
    sort_mass = [mass (1:ps)'];
    sort_mass = sortrows(sort_mass,1);
    
    a=2-iter*((2)/me);
    for l = 1 : num_FClevel-1
        N = size_each_level(l);
        level_mass = sort_mass(lb_level(l):ub_level(l),1);
        for n = 1 : N
            index_n = n + lb_level(l) - 1;
            index_n = sort_mass(index_n,2);
            if(rand < prob_forage)
                index_prey = fix(rand*(ub_level(l+1)+1-lb_level(l+1)))+lb_level(l+1)
                index_prey = (index_prey>ub_level(l+1))*ub_level(l+1)+(index_prey<=ub_level(l+1))*index_prey
                index_prey = sort_mass(index_prey,2)
%                 vel(index_n,:) = pos(index_n,:) + (pos(index_prey,:)-pos(index_n,:)).*(2*rand(1,D)-1);
                r1=rand();
                r2=rand(); 
                A1=2*a*r1-a; % Equation (3.3)
                C1=2*r2; % Equation (3.4)
                D_alpha = abs(C1*pos(index_prey,:)-pos(index_n,:));
                vel(index_n,:) = pos(index_prey,:)-A1*D_alpha;
            else
                index_interact = RouletteWheel(level_mass,1);
                index_mate = index_interact
                index_mate = index_mate+lb_level(l)-1
                index_mate = sort_mass(index_mate,2)
%                 vel(index_n,:) = pos(index_n,:) + (pos(index_mate,:)-pos(index_n,:)).*(2*rand(1,D)-1);
                r1=rand();
                r2=rand(); 
                A1=2*a*r1-a; % Equation (3.3)
                C1=2*r2; % Equation (3.4)
                D_alpha = abs(C1*pos(index_mate,:)-pos(index_n,:));
                vel(index_n,:) = pos(index_mate,:)-A1*D_alpha;
            end
        end
    end
    l = num_FClevel;
    N = size_each_level(l);
    level_mass = sort_mass(lb_level(l):ub_level(l),1);
    for n = 1 : N
        index_interact = RouletteWheel(level_mass,1);
        index_n = n + lb_level(l) - 1;
        index_n = sort_mass(index_n,2);
        index_mate = index_interact;
        index_mate = index_mate+lb_level(l)-1;
        index_mate = sort_mass(index_mate,2);
%         vel(index_n,:) = pos(index_n,:) + (pos(index_mate,:)-pos(index_n,:)).*(2*rand(1,D)-1);
        r1=rand();
        r2=rand(); 
        A1=2*a*r1-a; % Equation (3.3)
        C1=2*r2; % Equation (3.4)
        D_alpha = abs(C1*pos(index_mate,:)-pos(index_n,:));
        vel(index_n,:) = pos(index_mate,:)-A1*D_alpha;
    end
 
%     vel = ((vel>=vlb)&(vel<=vub)).*vel+(vel<vlb).*vlb+(vel>vub).*vub;
    pos = vel;
    pos = ((pos>=plb)&(pos<=pub)).*pos+(pos<plb).*plb+(pos>pub).*pub;
    
    fitness = feval(fhd,pos',varargin);
    sort_fitness = [fitness' (1:ps)'];
    sort_fitness = sortrows(sort_fitness, 1);
    fitbest = sort_fitness(1,1);
    fitworst = sort_fitness(ps,1);
    gbestindex = sort_fitness(1,2);
    gbest = pos(gbestindex,:);
    gbestval = fitbest;
    
    if(bool_Plot)
        XArray = [ XArray iter]; 
        YArray = [ YArray gbestval];
        set( haxes , 'XData' , XArray , 'YData' , YArray );
        drawnow
    end
end
